#!/usr/bin/env python

import rospy
from nav_msgs.msg import Path
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped

pose_path = Path()
odom_path = Path()

def odom_callback(data):
    global odom_path
    global odom_path_pub
    odom_path.header = data.header
    pose = PoseStamped()
    pose.header = data.header
    pose.pose = data.pose.pose
    odom_path.poses.append(data.pose)
    odom_path_pub.publish(odom_path)

def pose_callback(data):
    global pose_path
    global pose_path_pub
    pose_path.poses.append(data)
    pose_path_pub.publish(pose_path)

rospy.init_node('path_node')

pose_sub = rospy.Subscriber('/ndtpso/pose', PoseStamped, pose_callback, queue_size=1)
odom_sub = rospy.Subscriber('/odom', Odometry, odom_callback, queue_size=1)
pose_path_pub = rospy.Publisher('/path/pose', Path, queue_size=10)
odom_path_pub = rospy.Publisher('/path/odom', Path, queue_size=10)

if __name__ == '__main__':
    rospy.spin()
